#include "Services_QE.h"

volatile extern uint8 u8scheduler_counter;
volatile extern uint8 u8scheduler_flag;

/* Array of opcode instructions of the Erase/Program function in the HCS08 family */
volatile uint8 PGM[59]  =
{  
    0x87,0xC6,0x18,0x25,0xA5,0x10,0x27,0x08,0xC6,0x18,0x25,0xAA,0x10,0xC7,0x18,0x25, 
    0x9E,0xE6,0x01,0xF7,0xA6,0x20,0xC7,0x18,0x26,0x45,0x18,0x25,0xF6,0xAA,0x80,0xF7, 
    0x9D,0x9D,0x9D,0x9D,0x45,0x18,0x25,0xF6,0xF7,0xF6,0xA5,0x30,0x27,0x04,0xA6,0xFF, 
    0x20,0x07,0xC6,0x18,0x25,0xA5,0x40,0x27,0xF9,0x8A,0x81
};
/*
0x87     :         PSHA
0xC61825 :         LDA FSTAT    ; Load accumulator with Flash status register
0xA510   :         BIT 0x10     ; Check for previous access errors
0x2708   :         BEQ NOERROR  ; If there are no errors, jump 0x08 bytes
0xC61825 :         LDA FSTAT    ; Load accumulator with Flash status register
0xAA10   :         ORA 0x10     ; Clear error flag
0xC71825 :         STA FSTAT    ; Save cleared register
0x9EE601 :NOERROR: LDA          ; SP-indexed. Retrieve original Acc. value
0xF7     :         STA          ; Latch Address and Data buffers
0xA620   :         LDA 0x20     ; Load program byte command
0xC71826 :         STA FCMD     ; Set flash command register
0x451825 :         LDHX FSTAT  
0xF6     :         LDA          ; HX-indexed. Read Flash status register
0xAA80   :         ORA 0x80     ; Set FCBEF bit to launch command
0xF7     :         STA          ; Write Flash status register
0x9D     :         NOP          ; Wait for 4 bus cycles
0x9D     :         NOP
0x9D     :         NOP
0x9D     :         NOP
0x451825 :         NOP
0xF6     :         LDA         
0xF7     :         STA
0xF6     :         LDA          ; Clear FCBEF bit.
0xA530   :         BIT 0x30     ; Check for Access errors or Protection violations
0x2704   :         BEQ FLASHOK  ; If there was no error, jump 0x04 bytes
0xA6FF   :         LDA 0xFF     ; Load Acc. with error code
0x2007   :         BRA DONE                 
0xC61825 :FLASHOK: LDA 0x1825   
0xA504   :         BIT 0x40     ; Check status register for Command Complete bit
0x27F9   :         BEQ FLASHOK  ; If not complete, check again                 
0x84     :DONE:    TAP          ; Copy Acc. to CCR
0x81     :         RTS          ; Return from subroutine
 */                   
                      
void vfnSystemOptionsConfig (void)
{
    SOPT1 = SYS_CONFIG_1;
    SOPT2 = SYS_CONFIG_2;
}

void vfnSystemClockGating (void)
{
    SCGC1 = SYSTEM_CLOCK_GATING_1;
    SCGC2 = SYSTEM_CLOCK_GATING_2;   
}

void vfnConfigureScheduler (void)
{
    // Temporarily use a 16 divider to achieve a 64ms period
    // When wanting to return to a 4ms period, change prescaler to 1
    vfnRTC_PrescalerSelect(RTC_PRESCALER_1);
    // Use internal trimmed reference of 31.25 kHz
    vfnRTC_ClockSelect(RTC_CLK_INTERNAL);
    
    // Use a 125 modulo to achieve the desired 4ms period
    vfnRTC_SetModulo(125);
    vfnRTC_InterruptConfig (RTC_INTERRUPTS_ENABLED);
}

void vfnFlash_ConfigureClock (void)
{
	// Set flash clock at 200kHz for a 16MHz bus clock
	FCDIV |= 0x4A;
	
	//FCDIV_PRDIV8 = 1;	
	//FCDIV_FDIV = 10;	
}

void vfnFlash_Protect (void)
{
	FPROT_FPS = 0x59; // Protect range from 0x0_4200 to 0x0_FFFF	
	FPROT_FPOPEN = 1;
}

#pragma MESSAGE DISABLE C1805
uint8 u8FlashErase (uint16 u16FlashPage)
{
    uint8 u8temp;
    uint8 u8Result;

    DisableInterrupts;
 
    u8temp = Page_Erase (u16FlashPage);

    if ( u8temp == 0xFF )
    {
        /* Signal Error */
        u8Result = FLASH_ERROR;
    }
    else
    {	
    	/* Signal Ok */
    	u8Result = FLASH_OK;
    }
    
	// Remember to re-enable interrupts later
    //EnableInterrupts;

    return u8Result ;
}

uint8 u8FlashProgramByte (uint16 u16FlashPage, uint8 u8FlashByte)
{
    uint8 u8temp;
    uint8 u8Result = FLASH_OK;
    
    DisableInterrupts;
    
    u8temp = Program_Byte (u16FlashPage, u8FlashByte);
    
    if (u8temp == 0xFF)
    {
        /* Signal error */
        u8Result = FLASH_ERROR;
    }
    
    // Remember to re-enable interrupts later
    //EnableInterrupts;
    
    return u8Result;
}

#pragma INLINE
void vfnPulseDelay (void)
{
    asm
    {
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP
        NOP
    }
}

interrupt 24 void isrScheduler (void)
{   
    vfnRTC_ClearFlag();
    
    u8scheduler_counter++;

    u8scheduler_flag = (u8scheduler_counter & 0x07) + 1;

}
















